/*
* 时间：2021.07.22
* 代码来源：公众号【从0到1的点云】
* 功能：形态学滤波分离地面点
*/
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/progressive_morphological_filter.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/keypoints/uniform_sampling.h>
#include  <iostream>
#include  <thread>
#include  <chrono>
#include <string>
class timer
{
public:
  std::chrono::time_point<std::chrono::steady_clock> begin, end;
  std::chrono::duration<float> duration;
  std::string function_name;
  timer(std::string name) :function_name(name)
  {
    begin = std::chrono::high_resolution_clock::now();
  }
  ~timer()
  {
    end = std::chrono::high_resolution_clock::now();
    duration = end - begin;
    std::cout << "函数" << function_name << "消耗的时间是    " << duration.count()*1000.0f << "    毫秒！" << std::endl;
  }
};
void pf(const char* str)
{
  std::cout << str<<"\n" << std::endl;
}
int main()
{

  
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_groundpoints(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_non_groundpoints(new pcl::PointCloud<pcl::PointXYZ>);
    //输入数据
    pcl::PCDReader reader;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    reader.read<pcl::PointXYZ>("1.pcd", *input_cloud);
    pf("输入完成!");
    ////降采样
    pcl::PointCloud<int> sampled_indices;
    pcl::UniformSampling<pcl::PointXYZ> uniform_sampling;
    uniform_sampling.setInputCloud(input_cloud);
    uniform_sampling.setRadiusSearch(5);
    uniform_sampling.filter((*cloud));
  
    pf("开始计算！");

    {
      timer time("地面滤波");
    // 创建形态学滤波器对象
    pcl::ProgressiveMorphologicalFilter<pcl::PointXYZ> pmf;
    pmf.setInputCloud(cloud);
    // 设置过滤点最大的窗口尺寸
    pmf.setMaxWindowSize(20);
    // 设置计算高度阈值的斜率值
    pmf.setSlope(1.0f);
    // 设置初始高度参数被认为是地面点
    pmf.setInitialDistance(0.5f);
    // 设置被认为是地面点的最大高度
    pmf.setMaxDistance(3.0f);
    pcl::PointIndicesPtr ground(new pcl::PointIndices);
    pmf.extract(ground->indices);
    // Create the filtering object
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(cloud);
    extract.setIndices(ground);
    extract.filter(*cloud_groundpoints);
    // 提取非地面点
    extract.setNegative(true);
    extract.filter(*cloud_non_groundpoints);
  }
    pf("计算完成！开始可视化！");
  //可视化
  pcl::visualization::PCLVisualizer viewer("地面滤波");
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(cloud_groundpoints, 0, 255, 0);
  viewer.addPointCloud<pcl::PointXYZ>(cloud_groundpoints, single_color1, "cloud_groundpoints");
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud_non_groundpoints, 255, 0, 0);
  viewer.addPointCloud<pcl::PointXYZ>(cloud_non_groundpoints, single_color2, "cloud_non_groundpoints");


  viewer.spin();
  return (0);
}
